#ifndef robina_tiny_communication_QNODE_HPP_
#define robina_tiny_communication_QNODE_HPP_

#include <ros/ros.h>
#include <string>
#include <QThread>
#include <QStringListModel>
#include <QByteArray>
#include <actionlib/server/simple_action_server.h>
#include <robina_tiny_communication/tiny_spotAction.h>
#include <robina_tiny_communication/tiny_2dnav_markerAction.h>
#include <robina_tiny_communication/tiny_2dnav_posAction.h>
#include <robina_tiny_communication/tiny_go_and_spot_posAction.h>
#include <robina_tiny_communication/tiny_go_and_spot_markerAction.h>
#include <robina_tiny_communication/tiny_introductionAction.h>
#include <QUdpSocket>
#include <QXmlStreamWriter>
#include <QXmlStreamReader>
#include <QDebug>

typedef actionlib::SimpleActionServer <robina_tiny_communication::tiny_2dnav_markerAction>          tiny_2dNav_marker_server;
typedef actionlib::SimpleActionServer <robina_tiny_communication::tiny_2dnav_posAction>             tiny_2dNav_pos_server;
typedef actionlib::SimpleActionServer <robina_tiny_communication::tiny_spotAction>                  tiny_spot_server;
typedef actionlib::SimpleActionServer <robina_tiny_communication::tiny_go_and_spot_posAction>       tiny_go_and_spot_server;
typedef actionlib::SimpleActionServer <robina_tiny_communication::tiny_go_and_spot_markerAction>    tiny_go_and_spot_marker_server;
typedef actionlib::SimpleActionServer <robina_tiny_communication::tiny_introductionAction>          tiny_introduction_server;

using namespace std;

namespace robina_tiny_communication
{
    class QNode : public QThread
    {
        Q_OBJECT

    public:
        QNode(int argc, char** argv,QHostAddress _ip, int _send_port, int _receive_port);
        ~QNode();

        bool init();

        void run();

    Q_SIGNALS:
        void rosShutDown();

    private:

        bool       parseXml                 ( QByteArray );
        bool       getResult                ( );
        void       sendXml                  ( QByteArray xml);

        QByteArray generate_2dNavMarkerXml  ( QString marker_goal );
        QByteArray generate_2dNavGoalXml    ( geometry_msgs::PoseStamped poseStamped );
        QByteArray generate_setSpotXml      ( bool enable );
        QByteArray generate_introductionXml ( QString introduction );
        QByteArray generate_armGoToHomeXml  ();
        QByteArray readPendingDatagrams     ();

        void tiny_2dNav_marker_executeCb        ( const robina_tiny_communication::tiny_2dnav_markerGoalConstPtr &goal );
        void tiny_2dNav_pos_executeCb           ( const robina_tiny_communication::tiny_2dnav_posGoalConstPtr    &goal );
        void tiny_spot_executeCb                ( const robina_tiny_communication::tiny_spotGoalConstPtr         &goal );
        void tiny_go_and_spot_executeCb         ( const robina_tiny_communication::tiny_go_and_spot_posGoalConstPtr &goal );
        void tiny_go_and_spot_marker_executeCb  ( const robina_tiny_communication::tiny_go_and_spot_markerGoalConstPtr &goal );
        void tiny_introduction_executeCb        ( const robina_tiny_communication::tiny_introductionGoalConstPtr &goal);

        enum LogLevel
        {
            Debug,
            Info,
            Warn,
            Error,
            Fatal
        };

    private:
        char** init_argv;

	int init_argc;
        int send_port;
        int receive_port;

        QUdpSocket *udpSocket;
        QUdpSocket *resultUdpSocket;
        QHostAddress ip;


        tiny_2dNav_marker_server        *tiny_2dNav_marker_as;
        tiny_2dNav_pos_server           *tiny_2dNav_pos_as;
        tiny_spot_server                *tiny_spot_as;
        tiny_go_and_spot_server         *tiny_go_and_spot_as;
        tiny_go_and_spot_marker_server  *tiny_go_and_spot_marker_as;
        tiny_introduction_server        *tiny_introduction_ac;

        std::string marker;
        geometry_msgs::PoseStamped pos;

        bool spot;
    };
}
#endif
